#include "steer.h"
#include "placemedicine.h"
/* Initialize steer */
void steer_init()
{
    // 30~50 ?
    steer_set_angle(GRIP_STEER_LEFT , LEFT_GRIP_CLOSE_ANGLE);
    HAL_TIM_PWM_Start(STEER_TIM, GRIP_STEER_LEFT);

    // 35~70 ?
    steer_set_angle(GRIP_STEER_RIGHT, RIGHT_GRIP_CLOSE_ANGLE);
    HAL_TIM_PWM_Start(STEER_TIM, GRIP_STEER_RIGHT);

    // 108~80
    steer_set_angle(BOX_STEER_LEFT  , LEFT_BOX_CLOSE_ANGLE);
    HAL_TIM_PWM_Start(STEER_TIM, BOX_STEER_LEFT);

    // 100~72
    steer_set_angle(BOX_STEER_RIGHT , RIGHT_BOX_CLOSE_ANGLE);
    HAL_TIM_PWM_Start(STEER_TIM, BOX_STEER_RIGHT);
}

/* Set angle (0~180) */
uint32_t steer_set_angle(steer_t steer, uint32_t angle)
{
    if(angle < STEER_MIN_ANGLE || angle > STEER_MAX_ANGLE)
        return 0;

    __HAL_TIM_SET_COMPARE(STEER_TIM, steer, ANGLE_TO_COMPARE(angle));
    return 1;

    // __HAL_TIM_SET_COMPARE(STEER_TIM, steer, angle);
    // return 1;
}

/* Get angle (0~180) */
uint32_t steer_get_angle(steer_t steer)
{
    uint32_t compare = __HAL_TIM_GET_COMPARE(STEER_TIM, steer);

    // return compare;

    if(compare <= STEER_MIN_COMPARE)
        return STEER_MIN_ANGLE;
    else if(compare >= STEER_MAX_COMPARE)
        return STEER_MAX_ANGLE;
    else
        return COMPARE_TO_ANGLE(compare);
}